import os
import sys
import time
import math
import select
import tty
import termios

from onboardDevSDK.drone_api import *

help_msg = """
GCS Controller!
---------------------------
h   : help message
q   : to quit

[   : arm
]   : disarm
t   : takeoff
l   : land
w   : fly forward
s   : fly backward
a   : fly left
d   : fly right

j   : fly high
m   : fly low
n   : turn left
,   : turn right
"""
###################################################################
# get key pressed
###################################################################
settings = termios.tcgetattr(sys.stdin)


def getKey():
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key


###################################################################
# 连接飞控，其他连接方式请参考demo.py
# 这里连接gazebo仿真
dev_addr = "udp://:14540@127.0.0.1:14580"
params = {'sys_id': 1,
          'cmp_id': 200,
          'tgt_sys_id': 1,
          'tgt_cmp_id': 1,
          'fcu_url': dev_addr
          }

drone = DroneAPI(params)


def main():
    print(help_msg)
    velocity = 1
    turn_rate = 0.2
    while True:
        key = getKey()

        if key == 'h':
            print(help_msg)
        elif key == 'q':
            drone.exit()
            sys.exit(0)
        elif key == 'e':
            print(drone)

        elif key == '[':
            drone.arm()
        elif key == ']':
            drone.disarm()
        elif key == 't':
            print('takeoff')
            drone.takeoff()
        elif key == 'l':
            print('land')
            drone.land()

        elif key == 'w':
            drone.offboard_enable()
            drone.fly_front(velocity)
        elif key == 's':
            drone.offboard_enable()
            drone.fly_back(velocity)
        elif key == 'a':
            drone.offboard_enable()
            drone.fly_left(velocity)
        elif key == 'd':
            drone.offboard_enable()
            drone.fly_right(velocity)

        elif key == 'j':
            drone.offboard_enable()
            drone.fly_higher(velocity)
        elif key == 'm':
            drone.offboard_enable()
            drone.fly_lower(velocity)

        elif key == 'n':
            drone.offboard_enable()
            drone.turn_left(turn_rate)
        elif key == ',':
            drone.offboard_enable()
            drone.turn_right(turn_rate)


if __name__ == '__main__':
    print("INFO: run demo_keyboard_control.py")
    main()
